// #define DONOT_USE_RSEQ
#define _GNU_SOURCE
#include <assert.h>
#include <stdlib.h>
#include <libapfsds.h>
#include <ddblk_plat.h>
#include <dbg.h>
#include <dbg/ptest.h>
#include <pthreadalt.h>

#include <add.h>
#include <htable.h>
#include <nop.h>
#include <queue.h>
#include <stack.h>
#include <rbtree.h>
#include <tlsf.h>

#ifndef HDEF_LITTLECORE_OFFST
	#warning "undefined LITTLECORE_OFFST"
	#define HDEF_LITTLECORE_OFFST	0
#endif


#define TEST_NR		(1000000)
#define TEST_LOCKS	(1)

/* 执行线程数 */
/* 1, 2, 3, 4, 5, 6, 7, 8, 12, 16, 20, 24 */
/* 1, 2, 4, 8, 12, 16, 20, 24, 36, 48, 60, 72 */
#ifndef TEST_THREADNR
	#define TEST_THREADNR	(HDEF_NATIVE_CPU_NR * 3)
	#if (HDEF_NATIVE_CPU_NR == 8)
		#define WARMUP_THREAD_NR	(16)
		static word_t test_nrs_arr[] = {1, 2, 3, 4, 5, 6, 7, 8, 12, 16, 20, 24};
	#elif (HDEF_NATIVE_CPU_NR == 16)
		#define WARMUP_THREAD_NR	(16)
		static word_t test_nrs_arr[] = {1, 2, 4, 6, 8, 11, 13, 16, 24, 32, 40, 48};
	#elif (HDEF_NATIVE_CPU_NR == 24)
		#define WARMUP_THREAD_NR	(24)
		static word_t test_nrs_arr[] = {1, 2, 4, 8, 12, 16, 20, 24, 36, 48, 60, 72};
	#endif
#endif

#ifdef __aarch64__
	#define MAX_CYCLES	(16384)
	#define CYCLE_INTV	(32)
#else
	#define MAX_CYCLES	(3 * 1024 * 1024)
	#define CYCLE_INTV	(6144)
#endif

/* 采样记录WCET */

static TLS_INITIAL tid_t mytid;

#ifdef RECORD_WCET
NEW_CYCREC(sync, MAX_CYCLES, CYCLE_INTV)
PAGE_ALIGNED static cycrec_sync_t allsync_cycrec[TEST_THREADNR];
#define SAMPLING_PERIORD	(8)
SAI apfsds_return_t apfsds_sync_wcet_wrapper(apfsds_t *l, apfsds_func_t *func, apfsds_arg_t args){
	static TLS_INITIAL word_t sampling_cnt = 0;
	/* __DEBUG_ASSERT(mytid < TEST_THREADNR); */
	cyclecnt_t cycle0;
	cyclecnt_t diff;
	int ready = sampling_cnt++ % SAMPLING_PERIORD;
	if (ready){
		cycle0 = cyclestamp_get();
	}
	apfsds_return_t ret = apfsds_sync(l, func, &args);
	if (ready){
		diff = cyclestamp_diff(cycle0);
		cycrec_record_sync(&allsync_cycrec[mytid], diff);
	}
	return ret;
}
double percentiles[] = {0.9, 0.99, 0.999, 0.9999, 0.99999, 0.999999}; 
static word_t sync_wcetsum = 0;
SAI void sync_wcet_foreach_print(word_t idx, word_t cnt){
	sync_wcetsum += cnt;
	/* printf("[%8lu] %lu\n", (idx + 1) * CYCLE_INTV, cnt); */
}

SAI void sync_wcet_final_print(){
	for (word_t idx = 1; idx < TEST_THREADNR; idx++){
		cycrec_merge_sync(&allsync_cycrec[0], &allsync_cycrec[0], &allsync_cycrec[idx]);
	}
	/* 计算百分位数显示 */
	/* dump 到文件里? */
	sync_wcetsum = 0;
	CYCREC_FOREACH(allsync_cycrec[0], sync_wcet_foreach_print);

	/* 求出百分位数的值 */
	double cur_sum = 0;
	word_t percentile_res[sizeof(percentiles)/sizeof(double)] = {0};
	word_t cur_percentile_idx = sizeof(percentiles)/sizeof(double) - 1;
	for (word_t idx = MAX_CYCLES/CYCLE_INTV; idx > 0; idx--){
		cur_sum += allsync_cycrec[0].c[idx - 1];
	RESTART:
		if (cur_sum >= (double)sync_wcetsum * (1.0 - percentiles[cur_percentile_idx])){
			percentile_res[cur_percentile_idx] = idx * CYCLE_INTV;
			if(cur_percentile_idx == 0) break;
			cur_percentile_idx -= 1;
			goto RESTART;
		}
	}
	
	/* printf("percentiles\n"); */
	for (word_t idx = 0; idx < sizeof(percentiles)/sizeof(double); idx++){
		printf("%lf ", percentiles[idx]);
	}
	printf("\n");
	for (word_t idx = 0; idx < sizeof(percentiles)/sizeof(double); idx++){
		printf("%.0lf ", percentile_res[idx]/HDEF_CYCLE_FREQ_PERNS);
	}
	printf("\n");
}
#undef apfsds_sync
#define apfsds_sync(x, y, z)	apfsds_sync_wcet_wrapper(x, y, *(z))
#else
	SAI void sync_wcet_final_print(){}
#endif

static apfsds_t *apfsds[TEST_LOCKS];

static addcount_t addcnt;
static void *tadd(void *args){
	WRAP_ARGS(ar, &addcnt);
	word_t test_nr = (word_t)args;
	word_t ret = 0;
	for (;;)
	{
		ret = apfsds_sync(apfsds[0], count_add_cb, ar);
		if(ret >= test_nr)break;
	}
	return NULL;
}

static addcount_t addcnts[TEST_LOCKS];
static void *tadds(void *args)
{
	word_t test_nr = (word_t)args;
	/* word_t ret = 0; */
	word_t idx;
	word_t cnt = 0;
	for (;;)
	{
		idx = rdcnt_relax() % TEST_LOCKS;
		WRAP_ARGS(ar, &addcnts[idx]);
		apfsds_sync(apfsds[idx], count_add_cb, ar);
		if(++cnt >= test_nr)break;
	}
	return NULL;
}

static queue_t queue;
static void *tqueue(void *args){
	bool_t push = 0;
	word_t ret = 0;
	queuenode_t *pnode = (queuenode_t *)malloc(sizeof(queuenode_t));
	word_t test_nr = (word_t)args >> 1;
	pnode->val = (void *)rdcnt_relax();
	for (;;)
	{
		//printf("args %d\n",args);
		push = !push;
		if (push){
			WRAP_ARGS(ar, &queue, pnode);
			pnode = NULL;
			ret = apfsds_sync(apfsds[0], queue_push_tail_cb, ar);
		}else{
			WRAP_ARGS(ar, &queue);
			pnode = (void *)apfsds_sync(apfsds[0], queue_pop_head_cb, ar);
			pnode->val = args;
			if(!pnode) fatal("!!\n");
		}
		if(ret >= test_nr)break;
	}
	if (pnode)free(pnode);
	return NULL;
}

static sstack_t stack;
static void *tstack(void *args){
	stacknode_t *pnode = (stacknode_t *)malloc(sizeof(stacknode_t));
	bool_t push = 0;
	word_t ret = 0;
	pnode->val = (void *)rdcnt_relax();
	word_t test_nr = (word_t)args >> 1;
	for (;;)
	{
		//printf("args %d\n",args);
		push = !push;
		if (push){
			WRAP_ARGS(ar, &stack, pnode);
			pnode = NULL;
			ret = apfsds_sync(apfsds[0], stack_push_cb, ar);
		}else{
			WRAP_ARGS(ar, &stack);
			pnode = (void *)apfsds_sync(apfsds[0], stack_pop_cb, ar);
			if(!pnode) fatal("!!\n");
			pnode->val = args;
		}
		if(ret >= test_nr)break;
	}
	if (pnode)free(pnode);
	return NULL;
}

static htable_t *htable;
#define HTABLE_SIZE (HDEF_NATIVE_CPU_NR*HDEF_NATIVE_CPU_NR)
static void *thtable(void *args){
	static word_t keyid = HDEF_NATIVE_CPU_NR;
	word_t key = faa(&keyid, 1);
	hnode_t *pnode = (hnode_t *)malloc(sizeof(hnode_t));
	bool_t push = 0;
	word_t ret = 0;
	word_t test_nr = (word_t)args >> 1;
	for (;;)
	{

		push = !push;
		if (push){
			WRAP_ARGS(ar, htable, pnode);
			pnode->key = key;
			pnode = NULL;
			ret = apfsds_sync(apfsds[0], htable_insert_cb, ar);
		}else{
			WRAP_ARGS(ar, htable, key);
			pnode = (void *)apfsds_sync(apfsds[0], htable_remove_cb, ar);
			if (!pnode)
				fatal("!!\n");
		}
		if(ret >= test_nr)break;
	}
	if (pnode)free(pnode);
	return NULL;
}

static struct rb_root rbroot = {0};
static void *trbtree(void *args){
	
	static word_t keyid = HDEF_NATIVE_CPU_NR;
	word_t key = faa(&keyid, 1);
	struct mynode *pnode = (struct mynode *)malloc(sizeof(struct mynode));
	bool_t push = 0;
	word_t ret = 0;
	word_t test_nr = (word_t)args >> 1;
	for (;;)
	{

		push = !push;
		if (push){
			WRAP_ARGS(ar, &rbroot, pnode);
			pnode->key = key;
			pnode = NULL;
			ret = apfsds_sync(apfsds[0], rbtree_insert_cb, ar);
		}else{
			WRAP_ARGS(ar, &rbroot, key);
			pnode = (void *)apfsds_sync(apfsds[0], rbtree_remove_cb, ar);
			if (!pnode)
				fatal("!!\n");
		}
		if(ret >= test_nr)break;
	}
	if (pnode)free(pnode);
	return NULL;
}

#define MAX_ALLOCATION_SIZE (4096)
#define TLSF_POOL_SIZE (MAX_ALLOCATION_SIZE * 64)
static tlsf_t tlsf;
static void *ttlsf(void *args){
	bool_t malloc = 0;
	size_t size;
	void * mem = NULL;
	word_t ret = 0;
	word_t test_nr = (word_t)args >> 1;
	for (;;)
	{

		malloc = !malloc;
		if (malloc){
			size = hash(rdcnt_relax()) % MAX_ALLOCATION_SIZE;
			size |= 1; /* 确保size 至少为1 */
			WRAP_ARGS(ar, tlsf, size);
			mem = (void *)apfsds_sync(apfsds[0], tlsf_malloc_cb, ar);
			if (!mem) fatal("malloc error, size %lu\n", size);
		}else{
			WRAP_ARGS(ar, tlsf, mem);
			ret = apfsds_sync(apfsds[0], tlsf_free_cb, ar);
		}
		if(ret >= test_nr)break;
	}
	return NULL;
}
__PERF_PARALLEL_FUNC_BINDCORE_CUSTOM(tadd , mytid, TEST_NR, TEST_THREADNR, HDEF_LITTLECORE_OFFST, assert(addcnt.val >= TEST_NR); addcnt.val = 0, test_nrs_arr, dbg_printf)
// __PERF_PARALLEL_FUNC_BINDCORE_CUSTOM(tadds , TEST_NR, TEST_THREADNR, for (word_t idx = 0; idx < TEST_LOCKS; idx++){ addcnts[idx].val = 0; })
__PERF_PARALLEL_FUNC_BINDCORE_CUSTOM(tstack , mytid, TEST_NR, TEST_THREADNR, HDEF_LITTLECORE_OFFST, assert(stack.count >= (TEST_NR/2)); stack.count = 0, test_nrs_arr, dbg_printf)
__PERF_PARALLEL_FUNC_BINDCORE_CUSTOM(tqueue , mytid, TEST_NR, TEST_THREADNR, HDEF_LITTLECORE_OFFST, assert(queue.count >= (TEST_NR/2)); queue.count = 0, test_nrs_arr, dbg_printf)
__PERF_PARALLEL_FUNC_BINDCORE_CUSTOM(thtable , mytid, TEST_NR, TEST_THREADNR, HDEF_LITTLECORE_OFFST, assert(htable->count >= (TEST_NR/2)); htable->count = 0, test_nrs_arr, dbg_printf)
__PERF_PARALLEL_FUNC_BINDCORE_CUSTOM(trbtree , mytid, TEST_NR, TEST_THREADNR, HDEF_LITTLECORE_OFFST, assert(rbroot.count >= (TEST_NR/2)); rbroot.count = 0, test_nrs_arr, dbg_printf)
__PERF_PARALLEL_FUNC_BINDCORE_CUSTOM(ttlsf , mytid, TEST_NR, TEST_THREADNR, HDEF_LITTLECORE_OFFST, assert(((control_t *)tlsf)->free_count >= (TEST_NR/2)); ((control_t *)tlsf)->free_count = 0, test_nrs_arr, dbg_printf)





int main(){
	for (word_t idx = 0; idx < TEST_LOCKS; idx++){
		// printf("%d\n", idx);
		apfsds[idx] = apfsds_create();
	}
	
	count_init(&addcnt);
	for (word_t idx = 0; idx < TEST_LOCKS; idx++){
		count_init(&addcnts[idx]);
	}
	queue_init(&queue);
	stack_init(&stack);

	htable = htable_create(HTABLE_SIZE);
	for (word_t idx = 0; idx < HDEF_NATIVE_CPU_NR; idx++){
		hnode_t *node = (hnode_t *)malloc(sizeof(hnode_t));
		node->key = idx;
		htable_insert(htable, node);
	}

	my_rbinit(&rbroot);
	for (word_t idx = 0; idx < HDEF_NATIVE_CPU_NR; idx++){
		struct mynode *node = (struct mynode *)malloc(sizeof(struct mynode));
		node->key = idx;
		my_insert(&rbroot, node);
	}

	void *mem;
	int t = posix_memalign(&mem, 4096, TLSF_POOL_SIZE * HDEF_NATIVE_CPU_NR); // 8MB
	if (!mem || t != 0)
		fatal("tlsf memory alloc failed! %d\n", t);
	tlsf = tlsf_create_with_pool(mem, (size_t)(TLSF_POOL_SIZE * HDEF_NATIVE_CPU_NR));
	
	addcnt.val = TEST_NR;
	test_tadd();

	// test_tadds();
	stack.count = (TEST_NR/2);
	test_tstack();
	queue.count = (TEST_NR/2);
	test_tqueue();
	htable->count = (TEST_NR/2);
	test_thtable();
	rbroot.count = (TEST_NR/2);
	test_trbtree();
	((control_t *)tlsf)->free_count = (TEST_NR/2);
	test_ttlsf();

	for (word_t idx = 0; idx < TEST_LOCKS; idx++){
		apfsds_destroy(apfsds[idx]);
	}
	sync_wcet_final_print();
	// apds_destroy(apfsds);
	// printf("all test passed\n");
	return 0;
}